﻿using System.Collections.Generic;

namespace GeoFits
{
    /// <summary>
    /// 平面拟合
    /// </summary>
    public static class PlaneFit
    {
        /// <summary>
        /// 平面拟合
        /// </summary>
        /// <param name="pts">所要进行拟合的点的串</param>
        /// <param name="P">权阵</param>
        /// <param name="config">是否使用抗差最小二乘法</param>
        /// <returns>拟合得到的平面</returns>
        public static FitResult Fit(List<Point> pts, Matrix P = null, RLSConfig config = null)
        {
            Quaternion qt;
            //认为变化最小的一个方向法向量变化最大
            int n = FitHelper.getSmallestDiff(pts, true);
            switch (n)
            {                
                case 1:
                    //Y绝对值最小
                    qt = Quaternion.getQuaternion(new Vector(0, 1, 0), new Vector(1, 0, 0));
                    break;
                case 2:
                    qt = Quaternion.getQuaternion(new Vector(0, 0, 1), new Vector(1, 0, 0));
                    break;
                case 0:
                default:
                    //X绝对值最小，此时a=1
                    qt = Quaternion.getQuaternion(new Vector(1, 0, 0), new Vector(1, 0, 0));
                    break;
            }
            List<Point> newPts = Quaternion.Rotation(pts, qt);
            FitResult result = new FitResult();
            Matrix X = FitPattern.LinearModule(newPts, PlaneFitFunctionModule, out result.sigma0, out result.Qxx, out result.Qvv, out result.V, P, config);
            Vector vt = new Vector(1, X[0, 0], X[1, 0]);
            Vector vtAftRt = qt.Inverse() * vt;
            result.ResultGeometry = new Plane(vtAftRt.X, vtAftRt.Y, vtAftRt.Z, X[2, 0]);
            return result;
        }
        #region 私有函数
        
        /// <summary>
        /// 平面拟合函数模型生成
        /// </summary>
        /// <param name="pts">点</param>
        /// <param name="A">系数阵</param>
        /// <param name="L">常数阵</param>
        /// <param name="m0">初始值（此例中未使用此值）</param>
        /// <param name="conditions">限制条件（此例中未使用此值）</param>
        private static void PlaneFitFunctionModule(List<Point> pts, out Matrix A, out Matrix L, Matrix m0, params double[] conditions)
        {
            int n = pts.Count;
            A = new Matrix(n, 3);
            L = new Matrix(n, 1);

            for (int i = 0; i < n; i++)
            {
                A[i, 0] = pts[i].Y;
                A[i, 1] = pts[i].Z;
                A[i, 2] = 1;
                L[i, 0] = -pts[i].X;
            }
        }
        #endregion
    }
}
